#include <Servo.h>
#include <robotLac.h>
char INBYTE; //Entrada del bluetooth
robotLac robot(28,1);
Distance_Sensor dSensor(31, 29);
int llego=0;

void Seguirparedder();
void Seguirparedizq();

void setup(){
   Serial3.begin(9600);
  robot.begin();
  robot.detener();
  dSensor.begin();
}

void loop (){  
    
    llego=0;
     if(Serial3.available()){
    INBYTE = Serial3.read();
    Serial3.println(INBYTE);
    switch (INBYTE){
      case '1': Seguirparedder(); break;
      case '2': Seguirparedizq(); break;
      case '3': robot.detener(); break;
      
      delay(1000);
    }
  }
  
  

}



void Seguirparedder(){
while(!Serial3.available()){
 if((dSensor.get_distance()>0.20)&&(llego==0)){
    robot.avanzar();
    delay(100);
    }
  else if(dSensor.get_distance()>0.20){
    llego=1;
    robot.antihorario();
    delay(100);
  }
  else{
    llego=1;
    delay(200);
    robot.derecha();
    delay(100);
    
    if(dSensor.get_distance()<0.10){
    robot.horario();
    delay(300);
  }
  }  }
}




//void Seguirparedizq(){
//while(!Serial3.available()){
// if((dSensor.get_distance()>0.20)&&(llego==0)){
//    robot.avanzar();
//    delay(100);
//    }
//  else if(dSensor.get_distance()>0.20){
//    llego=1;
//    robot.horario();
//    delay(100);
//  }
//  else{
//    llego=1;
//    delay(200);
//    robot.izquierda();
//    delay(100);
//    
//    if(dSensor.get_distance()<0.10){
//    robot.antihorario();
//    delay(300);
//  }
//  }  
//}
//}

void Seguirparedizq(){
while(!Serial3.available()){
 if((dSensor.get_distance()>0.20)&&(llego==0)){
    robot.avanzar();
    delay(100);
    }
   else{
     
      llego=1;
    delay(200);
    robot.izquierda();
    delay(100);
     
   while(dSensor.get_distance()>0.20){
    llego=1;
    robot.horario();
    delay(100);
     }
    while(dSensor.get_distance()<0.10){
    robot.antihorario();
    delay(300);
  }
  }  
}
}
